/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_YH50.c
 *
 *  Created on: 2022年8月7日
 *      Author: 64435
 */

#include "LOS_YH50.h"
#include "LOS_OSAPI.h"
#include "LOS_Task.h"

LOS_YH50STA_t STA_YH50;
LOS_YH50TEST_t TEST_YH50;

uint8_t TMPACK_YH50[32] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

//X Y Z轴标度因数
float YH50_ScaleFactor[3] = {16670.6616, 16736.5655, 16675.5650};

void LOS_YH50_SendCMD(uint16_t cmd)
{
    uint8_t data[4]={0xEB, 0x00, 0x00, 0x00};
    LOS_MAX14830_ResetFIFO(YH50_MAXNUM, YH50_MAXPort);
    memcpy(&data[1], &cmd, 2);
    data[3]= data[1] + data[2];
    LOS_MAX14830_WriteBytes(YH50_MAXNUM, YH50_MAXPort, data ,4);
    STA_YH50.HL_TXF = 1;
    //LOS_Task_Resume(Task_MAX14830UART_Handle);
}



uint8_t LOS_YH50_DataProcess(uint8_t* data)
{
    double vel_deg[3] = {0, 0, 0};
    double vel_rad[3] = {0, 0, 0};
    uint8_t vel_buf[4] = {0, 0, 0, 0};
    uint32_t x_vel = 0;
    uint32_t y_vel = 0;
    uint32_t z_vel = 0;
    int32_t x_vel_s = 0;
    int32_t y_vel_s = 0;
    int32_t z_vel_s = 0;
    int16_t lum_pow = 0;
    int16_t tempbuf = 0;
    uint8_t checksum = data[1]+data[2]+data[3]+data[4]+data[5]+data[6]+data[7]+data[8]+data[9]+data[10]+data[11]+data[12]+data[13]+data[14]+data[15]+data[16]+data[17]+data[18]+data[19]+data[20];


    if (data[0] != 0x14)
    {
        STA_YH50.VAL = 0;
        return 1; //帧头错误，直接返回
    }
    else
    {
        STA_YH50.VAL = data[1];
        STA_YH50.Flag_D = 1;
        //光功率原始值
        memcpy(&lum_pow, &data[3], 2);
        STA_YH50.lumpow = lum_pow;

        //x轴角速度
        vel_buf[0]=0X00;
        vel_buf[1] = data[5];
        vel_buf[2] = data[6];
        vel_buf[3] = data[7];
        memcpy(&x_vel,vel_buf,4);
        x_vel = x_vel >> 23;
        if (x_vel == 1)
        {
            vel_buf[0]=0xFF;
            memcpy(&x_vel,vel_buf,4);
        } 
        else
        {
            vel_buf[0]=0X00;
            memcpy(&x_vel,vel_buf,4);
        }
        memcpy(&x_vel_s,&x_vel,4);
        STA_YH50.x_gyro = x_vel_s / 16670.6616;
        STA_YH50.x_gyro_deg = STA_YH50.x_gyro;
        STA_YH50.x_gyro = (STA_YH50.x_gyro/(double)180)*3.1415926;  //角度转弧度

        //y轴角速度
        vel_buf[0]=0X00;
        vel_buf[1] = data[8];
        vel_buf[2] = data[9];
        vel_buf[3] = data[10];
        memcpy(&y_vel,vel_buf,4);
        y_vel = y_vel >> 23;
        if (y_vel == 1) //为负数
        {
            vel_buf[0]=0xFF;
            memcpy(&y_vel,vel_buf,4);
        } 
        else
        {
            vel_buf[0]=0X00;
            memcpy(&y_vel,vel_buf,4);
        }
        memcpy(&y_vel_s,&y_vel,4);
        STA_YH50.y_gyro = y_vel_s / 16736.565533;
        STA_YH50.y_gyro_deg = STA_YH50.y_gyro;
        STA_YH50.y_gyro = (STA_YH50.y_gyro/(double)180)*3.1415926;

        //z轴角速度
        vel_buf[0]=0X00;
        vel_buf[1] = data[11];
        vel_buf[2] = data[12];
        vel_buf[3] = data[13];
        memcpy(&z_vel,vel_buf,4);
        z_vel = z_vel >> 23;
        if (z_vel == 1) //为负数
        {
            vel_buf[0]=0xFF;
            memcpy(&z_vel,vel_buf,4);
        } 
        else
        {
            vel_buf[0]=0X00;
            memcpy(&z_vel,vel_buf,4);
        }
        memcpy(&z_vel_s,&z_vel,4);
        STA_YH50.z_gyro = z_vel_s / 16675.563233;
        STA_YH50.z_gyro_deg = STA_YH50.z_gyro;
        STA_YH50.z_gyro = (STA_YH50.z_gyro/(double)180)*3.1415926;

        //X轴温度
        memcpy(&tempbuf, &data[14], 2);
        STA_YH50.x_temp = tempbuf;
        //Y轴温度
        memcpy(&tempbuf, &data[16], 2);
        STA_YH50.y_temp = tempbuf;
        //Z轴温度
        memcpy(&tempbuf, &data[18], 2);
        STA_YH50.z_temp = tempbuf;

        STA_YH50.HL_TXF = 0;
        //STA_YH50.TMCNT++;
        return 0;
        
    }
}


void LOS_YH50_PWROn()
{
    LOS_EPS_TCSwitch(16, SWITCH_ON);
    STA_YH50.CMD = 1;
}

void LOS_YH50_PWROff()
{
    LOS_EPS_TCSwitch(16, SWITCH_OFF);
    STA_YH50.CMD = 0;
    STA_YH50.MSG = 1;
}

//   10000000 00000010 00000000
